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ABSTRACT 



An Autonomous Underwater Vehicle (AUV) can combine a 
Global Positioning System (GPS) receiver with an Inertial 
Navigation System (INS) to navigate with a specified 
accuracy level. The AUV would be required to surface 
periodically to obtain a GPS fix. A computer simulation has 
been developed using an AUV model and an INS error model to 
generate noisy measurements. A Kalman filter is used to 
estimate the simulated INS errors. Several runs were 
executed to compare combinations of equipment with different 
levels of accuracy. 
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INTRODUCTION 



An Autonomous Underwater Vehicle (AUV) is a submersible 
designed to operate independently of real-time human 
control. In order to carry on planned missions, such a 
vehicle requires a navigation system to determine its 
location at any time. For covert and long range operations, 
passive navigation is preferable, and has to be designed to 
provide sufficient accuracy during the entire mission. 

A passive and accurate navigation system can be 
developed by combining an Inertial Navigation System (INS) 
with a Global Positioning System (GPS) receiver in a 
complementary fashion. Since the errors in any INS grow 
with time, they can be corrected by programming the AUV to 
surface periodically to get a GPS fix. 

The GPS uses satellites to determine position and 
velocity. A measurement of the time-of-arrival of the GPS 
signal at the receiver is combined with knowledge of the 
satellite's position to estimate the range to the satellite 
and the user's clock error. This measurement is referred to 
as the "pseudorange” . Another measurement, called the 
"delta range”, is used to determine the user's velocity from 
the Doppler shift of the GPS carrier frequency. The 
accuracy of velocity determined by GPS is based on knowledge 
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of the orbital parameters of the satellites and the short 
carrier wavelength, which is approximately 19 cm [Ref. 1]. 

The accuracy of GPS position data depends on several 
factors, which are discussed in Chapter 2. There are 
differences, in terms of accuracy, between military and 
civilian applications, authorized and non-author ized users, 
respectively. An independent authorized user can determine 
position with a two-dimensional (2-D), horizontal accuracy 
of better than 17.8 meters [Ref. 2]. Non-authorized users 
can obtain commercially available equipment that provides 
independent position estimates with a 2-D accuracy on the 
order of 100 meters. If a reference station is available, 
then differential corrections can be used to provide 
accuracy of better than five meters for any user. 

An INS uses accelerometers to measure the forces acting 
on a vehicle, and gyroscopes ("gyros") to determine the 
direction of those forces. Vehicle accelerations can be 
derived from the measured forces. By integrating the 
accelerations we can compute velocities, and with a second 
integration we obtain the position relative to the initial 
conditions. An INS also outputs the orientation of the 
vehicle, because this attitude information is required when 
determining the force directions. 

The accuracy of INS data depends on the quality of the 
equipment. The trade-offs for better accuracy typically 
involve size, weight, and cost. 
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An INS can be used to measure a vehicle's trajectory. 

One problem with using an INS is the growth of errors over 
time due to the integration of acceleration. This drawback 
can be overcome by combining the INS with independent 
position and velocity measurements, in order to estimate and 
correct the INS errors . 

This thesis addresses the use of a combination of INS 
and GPS equipment for the navigation of an AUV. Since GPS 
signals cannot be received underwater, GPS position and 
velocity measurements are obtained by requiring the vehicle 
to surface periodically. The integration of INS and GPS 
data is implemented using. Kalman filtering techniques. 

The Kalman filter approach described in this research is 
based on statistical models of the errors inherent to INS 
and GPS measurements. In particular, the INS gives 
measurements of position, velocity, and angular orientation 
of the vehicle in different coordinate frames (inertial and 
body fixed) , while the GPS yields measurements of position 
and velocity in an earth-fixed frame. The two systems are 
combined as shown in Figure 1.1, where we can see that the 
Kalman filter attempts to determine an optimal estimate of 
the errors. The estimates are consequently used to correct 
one of the measurement sets (the INS, for example) . This 
configuration is an open-loop implementation. The task of 
determining error models is of fundamental importance in 
this problem. 
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P + Sp P + Sp 

V + Sv V f gv 




t - error 



NOTATION: Sp = INS position error ! 

ep = GPS position error i 

^ = INS position error estimate ' 

sp = sp - ^3 estimate error 
V = velocity 

Figure 1.1: Open-loop aiding of Inertial Navigation 

System (INS) measurements using Global Positioning System 
(GPS) data. (After Ref. 3:p. 266) 



The following approach can be used to determine the 
attitude errors. To obtain the error in pitch angle, a 
sample is taken when the depth rate sensor indicates zero 
vertical velocity. Assuming the AUV is in transition from a 
climb to a dive at this instant, the pitch angle is zero and 
the angle indicated by the INS is the error. The roll angle 
error is obtained by averaging samples of the roll angle 
indicated while the vehicle is near the surface. This error 
measurement is based on the assumption that the vehicle is 
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inherently stable and the roll angle will average zero 
during this interval. The heading error can be obtained by 
comparing a compass reading to the INS indication. 

Figure 1.2 illustrates the alternative closed-loop 
implementation, which feeds the error estimates back into 
the INS. The INS then uses the error estimates to compute 
corrected position, velocity, and attitude measurements. 

This leads to a filter that linearizes about an estimated 
trajectory that is updated with each aiding measurement. 

This approach is called extended Kalman filtering and is the 
preferred method for long duration missions on the order of 
weeks or more [Ref. 3:pp. 356-379]. 
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Both versions of GPS-aiding of an INS use position and 
velocity estimates from a Kalman filter within the GPS unit. 
The integration of GPS and INS measurements can be 
accomplished with a single Kalman filter, as illustrated in 
Figure 1.3 [Ref. 4]. 




Figure 1.3: Integrated GPS/ INS. (After Ref. 4) 



This thesis focuses on using the open-loop approach to 
aiding an INS with GPS. In this case the Kalman filter 
linearizes about the reference trajectory provided by the 
INS. Chapter II discusses GPS in terms of the available 
levels of accuracy and the procedure for determining 
position from the raw measurements. Some representative GPS 
data is included in chapter II to illustrate the typical 
errors inherent to the system. 
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In chapter III, the various types of INS's are described 
and background information is provided to clarify the 
terminology. The INS error model is developed in chapter 
IV, along with the Kalman filter equations. 

The computer simulation is described in chapter V, and 
the results of several runs are presented in chapter VI. 
Finally, chapter VII provides conclusions and 
recommendations for further work in this area. Software 
listings are included in the appendices, along with a 
bibliography, to assist with related research. The software 
has not been verified beyond the results documented here and 
further application is the responsibility of the user. 
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II 



GLOBAL POSITIONING SYSTEM 



A. LEVELS OP ACCURACY 

The GPS is a Department of Defense (DOD) satellite 
navigation system. The primary purpose of the GPS is to 
support military navigation requirements. Two levels of 
accuracy are being provided by the GPS in accordance with 
DOD policy. Authorized users, mostly military, are allowed 
to use the high-accuracy capabilities of the system. Some 
applications outside the military have been granted 
permission to use these capabilities. All other users are 
referred to as non-authorized users, but they have unlimited 
access to the low-accuracy capabilities. 

Authorized users are provided with a high-accuracy 
navigation system that is totally passive, i.e., it does not 
require radiating signals from the user. The advantages of 
non-radiating user equipment include less power required, 
smaller size units, and the ability to operate covertly. 
These advantages are all due to the absence of a 
transmitter. 

The use of the GPS by non-authorized users is being 
permitted at reduced accuracy levels. The techniques used 
for degrading the accuracy are referred to as Selective 
Availability (SA) . The actual accuracy levels provided can 
be adjusted by the DOD in accordance with policy. In fact. 
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SA can be turned off entirely. The current policy (April 
1992), states that two times the standard deviation of 
horizontal position will be less than or equal to 100 meters 
[Ref. 2]. The Greek letter "sigma" is commonly used to 
denote the standard deviation, so this measure of accuracy 
is referred to as the "two sigma" (2c) level. The 
horizontal position is the standard 2-D solution, because it 
is the usual type of position required by ships at sea. 

The authorized users will have access to the Precise 
Positioning Service (PPS) , whereas non-authorized users will 
be limited to the Standard Positioning Service (SPS) . The 
accuracy levels associated with SPS and PPS are compared in 
Table 2.1. 

Table 2.1; ACCURACY LEVELS OF THE STANDARD POSITIONING 
SERVICE (SPS) AND THE PRECISE POSITIONING SERVICE (PPS) 
(After Ref. 2) 





SPS 


PPS 


HORIZONTAL POSITION (2a) 


100 m 


17.8 m 


VERTICAL POSITION (2a) 


156 m 


27.7 m 


RECEIVER CLOCK SYNCHRONIZATION (la) 


167 ns 


100 ns 



Another difference between SPS and PPS, in addition to 
SA, is the signal structure. For authorized users, a 
Precision code (P-code) is broadcast on two frequencies, Lj 
and Iti. A Coarse/Acquisition code (C/A code ) on L, provides 
a coarse navigation capability to SPS users, as well as a 
quick acquisition capability for PPS users. The PPS's use 



9 



of two L-band frequencies, 1575.42 Mhz (L,) and 1227.6 Mhz 
(Lj ) , provides a means for correcting for ionospheric delays 
[Ref. 1]. 

In addition to navigation, other applications are being 
found for the GPS, including geodesy and synchronization. 
Geodetic surveys usually require collecting data at a 
stationary site for extended periods of time. Time 
synchronization systems involve placing a GPS antenna at a 
surveyed location and solving for the receiver clock error. 

Another application of receiving GPS signals at a 
surveyed site is for using differential corrections to 
improve the accuracy of a mobile unit's position solution. 
Using differential techniques, the accuracy available for 
all users can be better than five meters. 

The use of interferometric techniques have proven to 
further improve the accuracy available, but continuous 
tracking of the signal carriers is required. Continuous 
tracking is difficult for maneuvering aircraft and is 
unfeasible for submersibles. 

For both SPS and PPS, the accuracy of the GPS data is also 
dependent on the relative positions of the satellites and 
the user. The relationship of satellite geometry to 
position accuracy is explained in the next section. 

B. POSITION DETERMINATION 

This section develops the equations used to determine 
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position based on GPS measurements. For more detailed 
information, the reader is referred to [Ref. 1] and [Ref. 
3:pp. 409-423]. The notation used in the following 
development is consistent with [Ref. 3:pp. 409-423]. 

The measurement used for the determination of a 
receiver's position is the GPS signal's time-of-arrival . 

This observable is referred to as the pseudorange, because 
it includes a bias due to the receiver's clock offset. The 
pseudorange measurement, p, includes the noiseless 
pseudorange, , along with the measurement noise, , and 

time-correlated errors, |3p [Ref. 3:p.412]. 

The GPS satellites broadcast information that includes 
orbital parameters, referred to as ephemerides. These 
ephemerides are used to calculate the satellite's position, 
[Xj, Yj, Zj]^ for satellite i. Four measurements are required 
to solve for the user's position, x = [x, y, z]’^, and the 
receiver's clock offset, At. From [Ref. 3:p. 410] 

fi = yj(X^-x)^ + + c At 

fa = + C At ^2.1^ 

tj = )J(Xj-x)^ + + (Zj-z)^ + c At 

f4 = '^{X^-x)'^ + (y^-y)^ + + c At, 

where c is the speed of light. If the altitude is known, 
e.g. , a vessel is on the sea's surface, than only three 
pseudorange measurements are required. 

Most GPS receivers use Kalman filtering, which requires 
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the linearization of Equation 2.1. Linearizing about an 



approximate position, = [x<j, y^, z<,]^, requires the partial 
derivatives : 

Hi ^ 

^ti ^ 

sjix^-x^y + {YrVo)^ * <Zi~zy 

^ -iZj-z^) 



for i = 1,...,4 [Ref. 3:p. 421]. These partial derivatives 
are the direction cosines from the satellite to the 
approximated user's position. 

Subtracting the predicted pseudorange, t|> , from the 
measured pseudorange gives the noiseless measurement 
equation: 









Suffi Sfi 6fi 










6x 6y bz ^ 




■fl 




fl(x^) 




6f2 ^f2 Sf2 




Ax ' 


f2 








bx by bz 




Ay 


fa 








6^3 SnJfj 61II/3 




Az 


f4 




f4<X ) 




bx by bz ^ 




c A t 




0 _ 




6^4 Sf4 6^4 










bx by bz 





where [Ax, Ay, Az]'^ = [Ref. 3:p. 422]. 

These measurements are based on the direction cosines. 
Therefore, the accuracy of the position estimates is 
dependent on the satellite geometry. 
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C. REPRESENTATIVE GPS DATA 



Data was collected with a GPS receiver to determine the 
statistics of position and velocity measurements. A C/A 
code receiver was used and therefore a comparison can be 
made with the published SPS position accuracies. 

The specific GPS receiver used was a Magnavox model MX 
4200 purchased by the Naval Postgraduate School. The data 
was stored on a laptop computer and software programs were 
written in FORTRAN to unpack the position and velocity 
measurements. Source code listings of these programs are 
provided in appendix A. A Microsoft FORTRAN compiler was 
used and these programs were run on IBM-compatible personal 
computers (XT and AT) . 

The GPS antenna was located on a surveyed antenna mount 
at Pt. Mugu, California. The surveyed position was 
subtracted from the measurements to obtain GPS errors. To 
convert from degrees of latitude and longitude to meters, 
correction factors were obtained from [Ref. 5]. 

The horizontal position errors are shown in Figure 2.1, 
and Figure 2.2 shows the vertical position errors. As seen 
in Figure 2.2, the vertical position error is constant for a 
period between sample 500 and sample 1000, this is due to 
the GPS receiver switching to an altitude-hold mode where 
only three satellites are used to determine the position. 
Figure 2.3 shows the velocity errors, which were converted 
from knots to meters per second. 



13 



The standard deviation of the horizontal position errors 
was computed to be 43.1 meters, which is close to the 
expected value of 50 meters based on the DOD policy. For 
the vertical position errors, the standard deviation was 
computed to be 81.7 meters, which again is close to the 
expected value of 78 meters. The velocity errors had a 
standard deviation of 0.9 meters per second. 



GPS doto collected 31 DEC 91 




Figure 2.1: Horizontal errors in GPS position measurements. 
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GPS doto collected 31 DEC 91 




Figure 2.2; Vertical errors in GPS position measurements. 



GPS doto collected 31 DEC 91 




Figure 2.3: Errors in GPS velocity measurements. 
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INERTIAL NAVIGATION SYSTEMS 



A. GIMBALLED SYSTEMS 

An INS measures accelerations and rotations in order to 
compute position, velocity, and orientation of a platform, 
usually a moving vehicle. A typical INS uses a triad of 
accelerometers, mounted on orthogonal axes, to measure the 
specific forces, f, experienced by a vehicle. Early systems 
mounted these accelerometers on a platform within an 
arrangement of gimbals. Sets of gyroscopes and servos were 
used to keep the platform stable within a reference 
coordinate system. Any rotation of the platform was sensed 
by the gyros and the servos would rotate the gimbal axes as 
required to counter the sensed rotation. These gimballed 
systems are still used and provide satisfactory accuracy at 
the expense of size, weight, and the low reliability 
associated with mechanical systems. 

A typical reference coordinate frame used for gimballed 
systems is the local-level frame ("n-frame") as illustrated 
in Figure 3.1. The coordinate axes are aligned with the 
local north, east, and down directions. The advantage of 
using this system is that the force of gravity is registered 
on only one of the accelerometers. This means that the 
forces sensed by the horizontal accelerometers are directly 
related to the vehicle accelerations in the familiar 
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directions of latitude, i|* / ^rid longitude, A. These 
accelerations can be integrated once to give velocities and 
again to provide changes in position from some starting 
location. 




A = longitude 



Yp = east 
Zp = down 



Figure 3.1; Reference frames used for inertial navigation 
systems. (After Ref. 6) 
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B. STRAPDOWN SYSTEMS 

With the development of the digital computer, an 
alternative configuration has become practical. By hard- 
mounting the accelerometers to the vehicle and using 
gyroscopes to measure rotations, (o) , a large part of the 
mechanical complexity can be reduced. The computer is 
required for keeping track of the orientation of the 
instrumentation package, so that the directions of the 
measured accelerations are known. Transformation matrices 
are used to convert vectors from one reference frame to 
another. For example, R"b transforms a vector in body frame 
("b-frame”) coordinates to n-frame coordinates. 

With these strapdown systems, the measurements are made 
in the b-frame, defined by the forward, right, and down 
directions [Ref. 7]. Thus, the rotation measurements give 
roll, pitch, and yaw directly. The problem is that, in 
addition to vehicle motion, each accelerometer measures 
components of other forces due to gravity and Coriolis, 
caused by the earth's rotation. These forces have to be 
estimated and subtracted, in order to obtain the 
accelerations due to vehicle motion. 

Figure 3.2 illustrates the process of converting the 
inertial measurements into attitude, position (<}>/ A,, h) , and 
velocity (v) . The results are in the local-level frame if 
the proper transformations from the b-frame to the n-frame 
are included in the processing. 
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Figure 3.2: Local-level frame mechanization of INS 

measurement processing. (From Ref. 8) 

An alternative INS mechanization, illustrated in Figure 
3.3, involves transformations from the b-frame to the earth- 
fixed frame ("e-frame”), labeled x, y, and z in Figure 3.1. 




Figure 3.3; Earth-fixed frame mechanization of an INS. 
(From Ref. 8) 



19 



It has been shown in [Ref. 8] that the processing of 
measurements using the e-frame can be accomplished faster 
than when using the n-frame. When using the e-frame, less 
computing time is required for the mechanization equations 
and the Kalman filter. More time is required for the 
computation of normal gravity, but the net result is faster 
processing with the e-frame algorithm. 

In this section we develop the equations for position 
and velocity estimates in the e-frame in terms of the forces 
acting on the vehicle. The equations are based on the 
developments in [Ref. 8] and [Ref. 9]. The reference frames 
used are the body frame (b) , the earth-fixed frame (e) , the 
local-level frame (n) , and the inertial frame (i) . Vectors 
and matrices are annotated with subscripts and superscripts 
depending on which frame or frames they reference. For 
instance, position and velocity in e-frame coordinates are 
denoted r! and v^ respectively. Similarly, is the skew- 

symmetric matrix of which is the angular velocity 

vector of the body frame with respect to the e-frame, given 
in b-frame coordinates. 

The skew-symmetric matrices are used to execute the 
cross-product operation. For instance, the Coriolis 
acceleration is two times the earth's rotation rate, Wg, 
crossed with the vehicle's velocity. 
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2(vt^k^) X + 2 



( 3 . 1 ) 



where the ^ denotes a unit vector. The skew-symmetric 
matrix serves to simplify the notation 



2 



0 -2Wg 0 




^ X 




-2 


2Ci)^ 0 0 






= 




O 

o 

o 








0 



(3.2) 



where 0 ®^^ is the skew- symmetric matrix of the angular 

velocity of the earth's rotation, 

The INS measurements are the specific force vector, f^, 
and the angular rotation rate of the body with respect to 
the inertial frame, Subtracting the earth's angular 

velocity vector, from gives 

= , (3.3) 

lb le ' ' 

which is used to build the skew-symmetric matrix 

K K (3.4) 

(q^^)3 0 -(«^^), , 

where (w*a2,)i is the roll rate, pitch rate, 

and ((a>*eij)3 is rate. 
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Accelerations are found by transforming the force 
measurements and subtracting the acceleration due to gravity 
and the Coriolis effect. The change in the transformation 
matrix from body to earth coordinates is given by 

. (3.5) 

The earth's angular velocity vector in body coordinates, 
is obtained from according to 

. (3.6) 

where the transformation matrix is the inverse of 

Transformation matrices involved are orthogonal, and 
therefore their inverses are equal to their transposes; 

. (3.7) 

We can relate different transformation matrices as [Ref. 6]: 

R\ . (3.8) 

where the transformation matrix, is obtained from 

initialization and the time history of body rotations. 

Using the geometry of Figure 3.1, and unit vectors 
along the e-frame and n-frame coordinate axes; 
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t- — 




> 




-sin<ijl) cosA - sin^ssinAj^ + cos4>^g 


Jn 


= R\ 


Je 


= 


-sinA fg + cosAjg 






A 




-cos<ij> cosA fg - cos<|> sinA - sin<|>^e 



where 4> denotes latitude and A denotes longitude. 
Therefore, 



R 



n 

e 



-sin'jjj cosA 
-sinA 
-cos4> cosA 



-sin<!j>sinA cos«|> 
cosA 0 

-cos4» sinA -sin<|i. 



( 3 . 10 ) 



and 



R 



e 

n 






-sin4»cosA -sinA -cos<t> cosA' 
-sin<t!i sinA cosA -cos4>sinA 
cosisjt 0 -sin<j!) 



( 3 . 11 ) 



After initialization, is obtained by integrating 



R\ = R% 



eb f 



1 . e, 



R\ik) 



k 



= R 



e 

b 



( 0 ) . 



0 



e 

b 




(Ac) 



( 3 . 12 ) 



where k is the number of the current sample and AC is the 
sampling interval. 

In summary, the mechanization equation is given by 



’ tl' 




Yl 




= 


R^t,£± - 2 + al 









where gf is the gravity vector. 
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C. GYROSCOPE PERFORMANCE CHARACTERISTICS 



The performance characteristics used for comparing gyros 
include gyro drift, scale factor error, and random walk 
noise. Other parameters of importance are the size, weight, 
cost, and dynamic range. The dynamic range is the range of 
input rotation rates that can be measured correctly. 

The gyro drift is also referred to as bias error or bias 
stability and is given in units of angle per unit of time 
(e.g., deg/sec). It is also common to describe a gyro's 
performance in terms of nautical miles (nmi) per hour. This 
measure is based on how the gyro would perform in an INS. 
Figure 3.4 shows how the position error grows with time for 
a given gyro drift. This plot shows that for a gyro bias of 
0.01 deg/hr, in combination with an accelerometer with a 
bias of 0.0001 m/s^, the performance is on the order of 1 
nmi /hr. 
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Figure 3.4 is based on a model given in [Ref. 10]. This 
model is valid for mission times of less than three hours. 
For missions of this duration, the Schuler oscillation is 
important, but the 24 hour effects can be neglected. The 
model is given by 

pit) = — ^(l-cosQ_t) + gri?-( t- — sinQ„t) 

^ , (3.14) 

v(t) = — sin<u)_t + gi?. (l-cosM)_t) 

iHg s a s 

where p(t) is the position (given as a function of time), 
v(t) is the velocity as a function of time, a is the 
accelerometer bias, g is the gyro drift, is the earth's 
radius, and u>g is the frequency of the Schuler oscillation. 

The scale factor is the resolution of the system. The 
scale factor error is given in parts per million (ppm) or 
percent (%) . Gyroscope noise is primarily due to random 
walk and is given in units of degrees per square root hour. 

System requirements are different depending on the 
application. A submarine for launching ballistic missiles 
can carry a large instrument package and requires high 
accuracy navigation. Since submarines remain submerged for 
extended periods of time, they cannot tolerate large error 
growth. Fortunately, these characteristics are compatible, 
i.e., for higher accuracy and smaller error growth, a large 
volume is required. 
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For commercial aircraft a medium-grade system is 
sufficient and usually preferred, due to the reduced cost. 
Such a system, with medium-level accuracy, is commonly 
referred to as an Attitude Heading Reference System (AHRS) . 
For a tactical missile with a short flight time, a low 
accuracy instrument is acceptable. 

Very high accuracy gyroscopes are used for scientific 
experiments related to geophysics and relativity. 

Geodesists and surveyors routinely use INS's, including 
those with Ring Laser Gyros (RLG's). Table 3.1 summarizes 
the main performance characteristics and provides typical 
value ranges for gyros of different levels of accuracy. 



Table 3.1; FAMI] 


LY OF GYROSCOPE 


APPLICATIONS (After Ref. 11) 


PERFORMANCE 


LOW-GRADE 


MEDIUM-GRADE 


HIGH-GRADE 


PARAMETER 


(munitions) 


(missiles) 


(submarines) 


Bias 


0.5 - 1 


0.1 - 1 


0.001 - 0.01 


Stability 


deg/sec 


deg/hr 


deg/hr 


Random Walk 
(deg/root hr) 


0.1 - 1 


0.02 - 0.25 


0.001 


Scale Factor 


0.1 - 5 % 


100 - 5000 


< 10 


Linearity 




ppm 


ppm 



D. OPTICAL GYROSCOPES 

1. Advantages of Optical Gyroscopes 

Prior to the development of the laser, mechanical 
gyroscopes were used extensively. These mechanical gyros 
consist of a spinning mass along with a motor to provide the 
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torque required to keep the mass spinning. Great care is 
taken during manufacturing to produce a balanced mass and 
reduce friction, but there is always some residual imbalance 
and friction contributing to measurement error. These 
mechanical devices are sensitive to large accelerations 
(g's) and have larger errors in a high-dynamic environment. 

The idea of using interferometry to measure rotation 
rates dates back to before the turn of the century, and 
Sagnac demonstrated this capability in 1913 during ether 
experiments [Ref. 12]. The first experiments with a laser 
gyro were performed in 1962 [Ref. 12]. Since that time the 
Ring Laser Gyro (RLG) has been developed into a practical 
instrument and is operational on aircraft ranging from the 
commercial Boeing 757/767 series [Ref. 13] to the military 
F-15 and UH-60 [Ref. 14]. Research and development of an 
operational Fiber Optic Gyro (FOG) is ongoing and flight 
tests have been conducted [Ref. 14]. 

The major advantage of optical gyroscopes is the 
large reduction in the number of moving parts. In fact, 
FOG'S are completely solid-state [Ref. 15]. This primary 
advantage provides several secondary benefits. With fewer 
moving parts, optical gyroscopes are easier to maintain, are 
more reliable, and are more rugged. In addition, for a 
given level of accuracy, optical gyros can be smaller and 
lighter, and have a lower total parts count. Components for 
FOG'S are readily available due to the increased use of 
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fiber optics in communication systems. All of these factors 
contribute to lower costs. 

Additional advantages are the large reductions in 
both warm-up time and sensitivity to high-dynamic maneuvers. 
According to [Ref. 15], FOG's are being developed for "smart 
munitions" which experience up to 20,000 g's when fired from 
a cannon. 

Optical gyros can be used to support navigation of 
platforms ranging from space vehicles to submarines. Indeed, 
this technology is currently replacing the traditional 
mechanical implementation. 

2. Theory of Operation: Sagnac Effect 

The Sagnac effect can be described [Ref. 16] in 
terms of the transit times of counter-propagating beams 
traveling in a circular optical cavity of radius R, as 
illustrated in Figure 3.5. The light enters the system from 
point A and the beam splitter diverts some of the light into 
the clockwise path and some of the light into the counter- 
clockwise path. If there is no rotation, then the beam 
splitter will not move and the two beams will recombine and 
return to point A. In this case, the transit time x is the 
same in both directions and is given by 



SAGNAC EFFECT 




Figure 3.5: Illustration of the Sagnac effect. 



When the sensor rotates at a rate of O rad/sec, the 
beam splitter moves a distance X from point A to point B, 
where 

X = i?Q-c . (3.16) 



For the beam traveling in the direction of the rotation, the 
transit time is 

2%R^X 2%R-^RQx 



X. = 



(3.17) 



For the oppositely directed beam, the transit time is 

2%R-RQx 



t_ = 



(3.18) 
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Factoring out the transit time and combining the two 
equations: 






2nR 



(3.19) 



The difference in transit times is 

2nR 2nR 



At = x_ = 



c-RQ c+RQ 



(3.20) 



Placing the two terms over a common denominator gives 

4nR^Q 



\x = 



c2-i?2Q2 



(3.21) 



Since : 



iX = 






(3.22) 



The optical path length difference is 

AL = cat = 



(3.23) 



For a circle of area A = itR^ : 



AL = 



4AQ 



(3.24) 



It can be shown that Equation 3.24 is valid for any 
geometric shape of area A. This equation shows how a 
rotation causes a change in optical path length. It also 
shows that increasing the area of the optical path will 
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increase the optical path length difference. For rotation 
rates of interest in most applications, an impractical area 
would be required if no alternatives were available. 
Fortunately, by using N turns of a fiber optic cable, the 
area is multiplied by N and practical systems can be built. 

The FOG is a passive interferometer and using a coil 
of N turns of cable provides the required sensitivity. A 
RLG is an active interferometer and its ability to provide 
the necessary sensitivity is described in the next section. 

3 . Ring Laser Gyros 

An active interferometer improves the sensitivity 
because the laser frequency depends on the cavity length 
[Ref. 16]. With a gain medium within the optical cavity, 
the condition for oscillation is that an integral number of 
wavelengths fits within the cavity length. In a ring laser 
the two oppositely traveling waves can have different 
amplitudes and frequencies. A small change in the optical 
path length leads to a small change in frequency given by 



_ AL 
V L 



(3.25) 



Since the optical frequency, v, is on the order of 
10'‘* Hz, small differences in length lead to changes in 
frequency that are large enough to be measured. From the 
Sagnac effect. 



AL = 



4A0 

c 



(3.26) 
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and the definition of optical frequency (v=c/i), the 
frequency difference is given by 



4AQ\ 



V 




L 



L 



c 



(3.27) 



Therefore, the beat frequency is: 




4A0 

LX 



(3.28) 



The net number of accumulated counts is found by 



integration: 




(3.29) 



where 0 is the net angle through which the gyro has turned. 
For example: given L = 39.6 cm, A = 75.45 cm^, and a He-Ne 

laser with a wavelength of 0.63 3 ixm, setting N = 1 gives a 
resolution (scale factor) of = 8.3 x lO"* rad = 1.7 

arcsec. This is equivalent to 7.6 x 10* counts per 
revolution or 2118 counts/deg. 



The read-out is obtained by using a prism behind one 



of the RLG's mirrors. The intensity of the resulting fringe 
pattern is given by 



where Aw is the angular beat frequency, 1/e is the fringe 



I = [l+cos (2ne/l+Aw t+<Js>] , 



(3 . 30) 
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spacing, x is the distance along the pattern, and 4> is an 
arbitrary phase angle [Ref. 16]. The fringes can be counted 
by using detectors smaller than the fringe spacing. Two 
detectors, separated by a quarter fringe, can be used to 
sense the direction of rotation. 

For an ideal RLG, the output N is a linear function 
of the input ©. The relationship between the input and 
output is referred to as the characteristic curve. Any 
deviation from the ideal case is considered a scale-factor 
error. The scale factor determines the slope of the 
characteristic curve. A phenomenon, referred to as mode 
pulling, changes the slope of the characteristic curve. 

Mode pulling is due to changes in the index of refraction 
within the gain medium. 

Another problem in RLG's is the lock-in due to back- 
scattering from the mirrors. At low rotation rates, a RLG 
will not respond. The typical method of overcoming lock-in 
is to mechanically dither the RLG, or its mirrors, using 
piezo-electric transducers so that the RLG operates outside 
of the problem area. The characteristic curve can also be 
shifted up or down so that a non-zero reading is obtained 
without any rotation. This effect is called null shift. 
Anything besides a rotation that influences the oppositely 
directed beams in an unequal manner will contribute to the 
null-shift effect. Such phenomena are known as 
nonreciprocal effects. 
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In order to get three RLG's with the largest area 
possible for a given volume, the three optical cavities can 
be combined within a single block [Ref. 16]. With this 
configuration, each mirror is shared by two of the optical 
paths, thus reducing the parts count [Ref. 17]. 
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IV. INS ERROR ESTIMATION 



A. INS ERROR MODEL 

The following development uses the techniques in [Ref. 

8] and [Ref. 18] to derive the equations governing the INS 
error dynamics. This INS error model is used in the Kalman 
filter to estimate the INS errors. There are 15 states 
based on five vectors. The first three states are the e- 
frame components of the attitude error, £.. The next six 
states are the e-frame components of the position error, 
ic, and the velocity error. Air. The last two vectors are 
the gyro drift, d, and the accelerometer bias, b, given in 
b-frame coordinates. 

The state vector has been augmented with the drift and 
bias terms, because they are modeled as Gauss-Markov 
processes. This augmentation has been done so that the 
system noise, w is white. The gyro drift noise and the 
accelerometer noise will have standard deviations of 

and Ojb, respectively. The resulting state vector [Ref. 8] 
for the error signal can then be written as 

= (l, Aj2, Ay, d, h)^ (4.1) 
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and it is modeled as a standard state space equation 

i: = [F] K + U, (4.2) 

where [F] is the dynamics matrix, determined on the basis of 
the system error equations. 

In particular, the misalignments are modeled as 

t = . (4.3) 

The position errors are only dependent on the velocity 
errors : 

^ = [I] hY, (4.4) 



where [I] is the identity matrix. 

The velocity errors can be affected by the normal 
gravity error. However the height of the AUV at the time of 
the measurement will be known to be sea level. Therefore, 
according to [Ref. 18], this error source can be neglected. 
We are left with the effects of the attitude errors coupled 
into the force measurements, the velocity errors coupled 
into the Coriolis force calculations, and the accelerometer 
biases. This is expressed by the equation 

M = -F® iL - 2 i , (4.5) 

where F' is the skew-symmetric matrix of the measured 
forces, f^, transformed into earth-fixed coordinates. 
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Modeling the gyro drifts and the accelerometer biases as 
first-order Gauss-Markov processes with C= 1 / (drift 
correlation time) and p= 1 / (bias correlation time) : 

d=~Cd * iC , (4.6) 

and 

= -pb + K . (4.7) 



The resulting 


model 


for 


the 


system 


errors then 


yields a 


dynamics matrix 




-Qe . 

“ le 


[0] 


[0] 




[0] 










[0] 


[0] 


[I] 


[0] 


[0] 




(4.8) 


[F] 


= 


-p e 


[0] 


-2Q« 


ie [0] 




/ 






[0] 


[0] 


[0] 


-C[J] 


[0] 










, [0] 


[0] 


[0] 


[0] 


-p[j]. 







where [0] is a three-by-three null matrix. 

B. KALMAM FILTER EQUATIONS 

Kalman filtering techniques can be used to estimate the 
INS errors based on the model given by Equations 4.1, 4.2, 
and 4.8. For the given AUV scenario. Equation 4.2 can be 
rewritten as 

X = [F] X + G Jir , (4.9) 

where G is the system noise matrix, and, from Equations 4.3 
through 4.7, 
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(4.10) 



[ 0 ] 


[ 0 ] 


[ 0 ] 


[ 0 ] 


[ 0 ] 


[ 0 ] 


[I] 


[ 0 ] 


[ 0 ] 





Standard techniques, such as those described in [Ref. 

3], can be used to discretize Equation 4.9. The resulting 
discrete version is 

xik+l) = ^(k)x(k) +T(k)i£{k) , (4.11) 

where T(k) is the discrete version of G, and w(k) is the 
white system noise vector. The transition matrix, ^(k) , 
relates the state vector at time t^, x(k) , to the state 
vector at the next sampling time, x(k+l) , under noiseless 
conditions . 

Estimates of the state vector, ^{k) , are obtained by 
filtering the measurements, ^(k) . At each sample time, t^, 
a new measurement is taken, which can be represented by 

Z(k) = H(k) z(k) + Yik) , (4.12) 

where H(k) is the observation matrix, and v(k) is the 
measurement noise. 

For GPS-aiding, the measurements are the errors in 
attitude, position, and velocity. Therefore, 
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(4.13) 



H = 



[I] [0] [0] [0] [0] 

[0] [I] [0] [0] [0] 

[0] [0] [I] [0] [0] 

[0] [0] [0] [0] [0] 

[0] [0] [0] [0] [0] 



ff 



and Y(k) consists of the noise on the aiding systems, 
including GPS, the depth meter, and the compass. 

In order to use a Kalman filter, the noise processes, 
w(k) and v(k) , must be uncorrelated. In addition, the non- 
zero, diagonal components of the system and measurement 
noise covariance matrices, Q(k) and R(k) , must be known. 

For AUV applications, these conditions are met, but the 
covariances differ depending on the conditions and equipment 
used. The best accuracy is obtained with a high-grade INS 
aided by differential GPS. 

The recursive Kalman filter algorithm is [Ref. 3:p. 235] 

K(k) = p-{k)H'^[Hp-(k)H'^+R(k)]-^ 

^(k) = ^(k)+K{k) [zik)-H^{k)] 

Pik) = [[I]-K{k)H]p-{k) (4.14) 

p-(ic+l) = ^{k)P{k)V(k)+Q(k) 

^(k+1) =^{k)^{k) , 



where the minus sign superscript refers to conditions just 
prior to incorporating the measurement. The K(k) matrices 
are the Kalman gains and the P(k) matrices are the error 
covariance matrices, based on the estimation errors given by 

X(k) =Kik)-g(k) . (4.15) 
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This algorithm requires an initial estimate of P(0) and 
^(0) . Since the INS errors are zero mean, the initial 

estimates of the state variables, ^(0) , are zeros. The 

values used for the initial variances in the error 
covariance matrix, P'(0) , are based on a commercially 
available INS. This particular INS was designed for 
civilian aircraft and measures position with an accuracy of 
two nautical miles per hour (2a = 2 nmi/h) [Ref. 18:p.24]. 
The initial variances are listed in Table 4.1. 



Table 4.1: INITIAL VARIANCES IN THE ERROR COVARIANCE MATRIX 

(After Ref. 18:pp. 63-64) 



STATE VARIABLE 


INITIAL 


ERROR VARIANCE 


' L 


2.35 X 10-'' 


(radians) ^ 


is. 


400 


(meters)^ j 


Sv 


10* 


(meters/second) ^ 


d 


2.35 X 10** 


(radians/second) ^ 


b 


O 

OO 


(meters/second^)^ 
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V. COMPUTER SIMULATION 



A computer simulation has been conducted to verify the 
theory explained in the preceding sections. As illustrated 
in Figure 5.1, this simulation included three main parts: 
an AUV model, an INS error model, and a Kalman filter. The 
simulation software includes sections that perform 
additional functions, such as providing control inputs to 
the AUV model, and adding noise to the measurements. This 
software was developed using the MATLAB applications 
package. 



COMPUTER SIMULATION 



CONTROL 

INPUTS 



FORCES AND 



NOISY 








ROTATION RATES 




MEASUREMENTS 




AUV II 




INS 




KALMAN 


MODEL 




ERROR 




FILTER 






MODEL 







INS ERROR 
ESTIMATES 






Figure 5.1: Block diagram of computer simulation. 



An existing, nonlinear AUV model [Ref. 19:pp. 18-34] was 
used to create realistic sequences of forces and rotations. 
This model, AUV2, was written in the C software language and 
compiled for use with MATLAB. The AUV2 program is based on 
the parameters of the Naval Postgraduate School's second 
AUV. 
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The flight profile for the AUV consisted of an initial 
surface interval followed by a series of dives. The initial 
surface interval was a two-minute period during which the 
AUV remained near the surface for continuous GPS 
measurements. This was done to allow the Kalman filter to 
settle down. A reference depth of one half meter was used 
because the AUV2 model is only valid when the vehicle is 
submerged. The hydrodynamic equations are different for a 
vehicle on the surface. The reference depth was used in a 
feedback loop, along with a gain term, to set the angle of 
the AUV's bow and stern planes. Several trial runs were 
conducted to find a suitable value for the gain. 

Following the initial surface interval, the AUV model 
was given a sinusoidal input to simulate thirty dives. Each 
dive lasted for two minutes, and a GPS measurement was made 
at the end of each dive. 

The forces and rotations output from AUV2 are used by an 
INS error model to create noisy measurements. This error 
model is based on the equations in Chapter 4 and is 
illustrated in Figure 5.2. 

To initialize the transformation matrix, R%, it is 
assumed that the AUV is aligned with the local-level frame. 
Thus the transformation matrix from body to local-level 
coordinates, R\, is reduced to the identity matrix, [I], 
and, from Equation 3.7, we have R% = R®n. A latitude and 
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Figure 5.2: Block diagram of INS error model. 



longitude near Monterey, California was used to initialize 
in accordance with Equation 2.10. Equation 2.11, based 
on the body rotations, is used to update R\. 

The forces are used, along with R%, to update the error 
dynamics matrix, [F] . The system noise matrix, G, is then 
used, along with [F], to form the discrete-time version of 
the state equation. To convert Equation 4.9 into Equation 
4.11, the following approximations are useful [Ref. 3:p.224] 
and [Ref. 20]: 
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4,= [I] . 



r = [ [j] (At) + 



F(A t) ^ 
2 ! 



2 ! 

FMAt)^ 

3 ! 



(5.1) 



+ ...]G 



The MATLAB command "c2d” also converts the state 
equation from the continuous to the discrete version. A 
comparison was done that showed that the approximating 
equations ran faster than the "c2d" algorithm. Several runs 
were executed to find out how many terms were required in 
Equations 5.1 to reduce the approximation errors to 
negligible values. 

Using MATLAB' s random number generator, and specifying a 
normal distribution with a given standard deviation, system 
noise is added to create a simulated state vector. 

Similarly, measurement noise is added and the observation 
matrix is used to create a simulated measurement vector. 
These noisy measurements are then fed into the Kalman filter 
to estimate the simulated state vector. 

The values used for the standard deviations of the 
system noise were 0.01 deg/h for gyro drifts and 10 mgal for 
accelerometer biases [Ref. 18:p. 64]. These values were 
based on an INS that uses RLG's. Converting the units gives 

0.0l"\/ \llhour\ 

= o-xxu ^jLau/ a 

(5.2) 



)( — jL^\/ = 5x10"® rad/s 

^ \ hour j\ 180 /\ 36 00s/ 



o. = (lOxlO’^gaJ) 



‘ 0 . Ol/n/s^ ^ 
gal 



= l0'*m/ . 
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For the measurement noise, the standard deviations were 
based on GPS statistics for position and velocity, and two 
levels of accuracy for attitude measurements. For 
differential GPS, the standard deviations of position 
measurement noise, is 2.5 meters, and the standard 

deviation of velocity measurement noise, a^, is 0.05 meters 
per second. For the C/A code GPS simulation, = 50 m, and 
= 0.9 m/s. 

A standard deviation of 10"^ radians was used to simulate 
a high accuracy attitude measurement. A low accuracy 
simulation was run using a value of 0.1 radian. 

These standard deviations were squared to obtain the 
variances required for the measurement noise covariance 
matrix, R. Table 5.1 lists the system noise variances. 

Table 5.1; STATE VARIABLE VARIANCES (After Ref. 18 :p. 65) 



STATE VARIABLE 


SYSTEM NOISE VARIANCE 


attitude error 


2.3 5 X lO'*^ 


(radians) ^ 


position error 


0 


(meters)^ 


velocity error 


10-^ 


(meters/second) ^ 


gyro drift 


3 X 10-'^ 


(radians/second) ^ 


accelerometer bias 


3.7 X lO-'' 


(meters/second^) ^ 



A correlation time of 40 hours was used for the Gauss- 
Markov process, i.e., gyro drifts and accelerometer biases 
[Ref. 18:p. 65]. 

Several MATLAB command files ("m-files”) were generated 
to perform the separate tasks of the computer simulation. 
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These files are included in Appendix B. The file 
”auv2surf .m” calls the AUV2 model with control inputs to 
generate vehicle state outputs for the surface interval. 

The control inputs are bow and stern plane angle commands 
based on the difference between a reference depth of 0.5 
meters and the fedback depth output from AUV2 . The file 
•'auv2dive.m" calls AUV2 with a sinusoidal input to simulate 
a series of dives. The file "plotdpth.m” graphs the depth 
during the first five dives. 

The files "simlsurf .m” , "sim2surf .m", "simSsurf .m” , and 
"sim4surf .m" use AUV2 output states during the surface 
interval to simulate the INS errors and to generate error 
estimates with the Kalman filter. The only difference in 
these four files are the values used for the standard 
deviations of position, velocity, and attitude measurement 
noise. The file "plotl.m" graphs the results from the 
simulated surface interval runs. 

The file "simdives.m” uses the results from the surface 
runs and ”auv2dive.m” to simulate the series of dives. The 
file ”plot2.m" graphs the results of the dive runs. The 
file ”esterr.m” calculates and plots the INS error estimate 
errors following each simulated series of dives. 

After the simulation software was debugged, and 
observability was verified, four runs were executed to 
compare the combinations of differential or C/A code GPS 
with high-accuracy or low-accuracy attitude measurements. 
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Table 5.2 lists the combinations used for the simulations. 
The results of the four simulation runs are presented and 
discussed in the next chapter. 



Table 5.2: ACCURACY LEVELS DURING THE COMPUTER SIMULATIONS 



RUN # 


GPS 


ATTITUDE MEASUREMENTS 


1 


differential 


high accuracy 


2 


differential 


low accuracy 


3 


C/A code 


high accuracy 


4 


C/A code 


low accuracy 
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VI 



RESULTS 



The results from the four simulation runs have been 
graphed using MATLAB plotting commands. The AUV depth 
during the initial surface interval and the following series 
of dives were the same for all four runs. Figure 6.1 shows 
the depth during the surface interval. Figure 6.2 shows the 
depth during the first five dives. 
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Figures 6.3 through 6.17 show the results from the first 
run. Figures 6.18 through 6.62 show the results from the 
next three runs. The first five figures from each run show 
the output states during the surface interval. The next 
five figures show the output states during the dive series. 
These output states include the modelled INS errors and the 
INS error estimates from the Kalman filter. The dashed 
curves are the error estimates. The last five figures in 
each set show the error estimate errors. 
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Figure 6.5: First-run, surface-interval velocity error. 




Figure 6.6: First-run, surface-interval gyro drift. 
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Figure 6.7: First-run, surface-interval accelerometer bias. 
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Figure 6.8; First-run, dive-series attitude error. 




Figure 6.9; First-run, dive-series position error. 
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Figure 6.11: First-run^ dive-series gyro drift. 
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Figure 6.12; First-run, dive-series accelerometer bias, 
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Figure 6.13: First-run attitude error estimate error. 
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Figure 6.14; First-run position error estimate error. 
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Figure 6.15: First-run velocity error estimate error. 
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Figure 6.17: First-run accelerometer bias estimate error. 



s 

I 

s 




I 

Ss* 

S 



-S —2 



xlQ-g ATTITUDE ERROR 



M 

r' \ 


• •/ 
: J' 


S M 

\.N 1 
■ V 

V' 


1 » • ./V V 



"t*. •«oond« 



SO 1 OO 

tfm*. •«OOnd« 




••corida 



Figure 6.18: Second-run, surface-interval attitude error ♦ 
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Figure 6.19: Second-run, surface-interval position error. 
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Figure 6.20: Second-run ^ surface-interval velocity error. 
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Figure 6.21: Second-run, surface-interval gyro drift. 
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Figure 6.23: Second-run, dive-series attitude error, 




Figure 6.24: Second-run, dive-series position error. 
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Figure 6 ♦26; Second-run, dive-series gyro drift. 
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Figure 6.27: Second-run, dive-series accelerometer bias. 
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Figure 6.28: Second-run attitude error estimate error. 
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Figure 6.29: Second-run position error estimate error. 
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Figure 6.30: Second-run velocity error estimate error. 
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Figure 6.32: Second-run accelerometer bias estimate error. 
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Figure 6.35: Third-run, surface-interval velocity error. 




Figure 6.36: Third-run, surface-interval gyro drift. 
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Figure 6.38: Third-run, dive-series attitude error. 
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Figure 6.39: Third-run, dive-series position error. 
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Figure 6.41: Third-run, dive-series gyro drift, 
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Figure 6.42: Third-run, dive-series accelerometer bias. 
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Figure 6.43: Third-run attitude error estimate error. 
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Figure 6.44: Third-run position error estimate error. 
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Figure 6.45: Third-run velocity error estimate error. 
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Figure 6.47: Third-run accelerometer bias estimate error, 
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Figure 6.48: Fourth-run, surface-interval attitude error. 




Figure 6.49: Fourth-run, surface-interval position error 
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Figure 6»50: Fourth-run, surf ace- interval velocity error. 
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Figure 6.51: Fourth-run, surface-interval gyro drift. 
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Figure 6.53: Fourth-run, dive-series attitude error. 
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Figure 6.54: Fourth-run , dive-series position error. 
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Figure 6.55: Fourth-run, dive-series velocity error. 
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Figure 6,56: Fourth-run, dive-series gyro drift. 
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Figure 6.57: Fourth-run, dive-series accelerometer bias. 
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Figure 6.58: Fourth-run attitude error estimate error. 
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Figure 6.59: Fourth-run position error estimate error. 
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Figure 6.60: Fourth-run velocity error estimate error. 
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Conclusions based on these results are included in the 
next chapter. 
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VII 



CONCLUSIONS AND RECOMMENDATIONS 



This research has produced a computer model of the 
integration of GPS and INS measurements for the navigation 
of an AUV. Noisy INS measurements were simulated with a 
model based on a medium-grade INS that uses RLG's. Noisy 
GPS and attitude measurements were simulated and subtracted 
from the modeled INS measurements to simulate INS errors. 
Kalman filtering was used to estimate those errors. Four 
simulations were run using combinations of high and low 
accuracy GPS and attitude measurements. 

The results from the simulation runs matched the 
expectations. This can be observed by comparing Figure 6.9 
with Figure 3.4. The simulated position error after one 
hour is on the order of one nautical mile, which is what is 
predicted by the theoretical equations. 

The simulation shows that Kalman filtering can estimate 
the position and velocity errors to the expected accuracy 
levels. Figures 6.14, 6.15, 6.29, and 6.30 show that the 
accuracy of the position and velocity error estimates is 
independent of the accuracy of the attitude measurements. 

The computer model can be used to compare combinations 
of INS's and GPS equipment with different levels of 
accuracy. These comparisons might include low-grade or 
high-grade INS's and P-code GPS receivers. 
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The recommended approach to continuing this research 
would begin with a verification of the INS error model. 

This could be accomplished using the AUV in the pool at the 
Naval Postgraduate School. A trajectory measurement using 
the AUV's sonars could be used as a standard to determine 
typical INS errors. Simulations would need to be run with a 
profile compatible with the dimensions of the pool. 

Additional research could investigate implementation 
with closed-loop utilization of state estimates and 
smoothing for improved accuracy. 



71 



APPENDIX A. GPS DATA PROCESSING SOFTWARE 



unpack. for; 

c unpack mx4200 data 

c 

character* 11 Rtype 
character*14 fname, outfname 

write (*/'(a\)') ' What is the input file name? ' 

read (*,'(a)') fname 

open (1, file=fname, access= ' sequential ' , 

+status= ' old ' ) 

write (*,'(a\)') ' What is the output file name? ' 
read (*/'(a)') outfname 
open (2, f ile=outfname, status=' new' ) 
startTIME = 0 
incH = 0 
incLAT = 0 
incLONG = 0 
100 continue 

read ( 1 , ' (a) ' , end=500 , err=100) Rtype 
write ( * , ' (a) ' ) Rtype 
if (Rtype .eq. ' $PMVXG, 001 , ' ) then 
backspace 1 

read (1, 50,err=100) Rtype, ih, im, is, ideg, dmin, 
+ideglong, dminlong, alt , isrc 

50 format (a, 3i2 , lx, i2 , f6. 3 , 3x, i3 , f6. 3 , 3x, f7 . 1, lx, il) 
seconds = im * 60 + is 
if (startTIME .eg. 0) then 
startTIME = seconds 
ihO = ih 
endif 

if (ih .gt. ihO) incH = 3600 * (ih-ihO) 
itime = incH + (seconds - startTIME) + 1 
if (ideg .ne. 34) incLAT = 60 * (ideg - 34) 
delLAT = incLAT + (dmin - 6.733) 

if (ideglong .ne. 119) incLONG = 60*(ideglong - 119) 
delLONG = incLONG + (dminlong - 6.9564) 
delALT = alt - 12.71 

write (2,60) itime, delLAT, delLONG, delALT, isrc 
60 format ( lx, i6 , 5x, f 6 . 3 , 5x, f 6 . 3 , 5x, f 6 . 1, 5x, il) 
endif 
goto 100 
500 close (1) 
close (2) 
end 
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unpKvel.for; 



c Steve Nagengast 

c filename: unpkvel.for 

c 

c unpack mx4200 velocity data 

c 

character*!! Rtype 
character*!4 fname, outfname 

write (*/'(^\)') ' What is the input file name? ' 

read (*/'(a)') fname 

open (!, file=fname, access= ' sequential ' , 

+status=' old' ) 

write (*/'(a\)') ' What is the output file name? ' 

read (*/'(a)') outfname 

open (2, f ile=outfname, status=' new' ) 

!00 continue 

read (! , ' (a) ' , end=500) Rtype 
write (* , ' (a) ' ) Rtype 
if (Rtype .eg. ' $PMVXG, 0!!, ' ) then 
backspace ! 

read (!,50) Rtype, hdng, spd 
50 format (a, f5. !, !x, f5. !) 

write (2,60) hdng, spd 
60 format (!x, f 5 . ! , 5x, f 5 . !) 

endif 
goto !00 
500 close (!) 
close (2) 
end 
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APPENDIX B. COMPUTER SIMULATION SOFTWARE 



auv23urf .m: 

% Steve Nagengast % simulates AUV2 for initial 

% filename: auv2surf.m % two-minute surface interval 

% Creates output file from AUV2 model 

clear 

% initial surface interval (120 seconds = two minutes) 

desireddepth = 0.5; 
planegain = 0.008; 
idt = 1; 
rpm = 550; 

state(:,l) = zeros (12,1); 
for k=l:120/idt 

plane = planegain * (desireddepth - state(9,k)); 

if (plane > 0.7) plane = 0.7; end 

if (plane < -0.7) plane = -0.7; end 

inpt = [0; 0; plane; -plane; rpm]; 

oldstate = state (:,k); 

state(:,k+l) = auv2 (oldstate, inpt, idt); 
end 

% save and plot results from end of initial surface interval 
save srfstate state 
plot (-state (9 , : ) ) 



auv2dive.m; 



% Steve Nagengast % simulates AUV2 thru thirty dives 

% filename: auv2dive.m % 

% reload last state from initial surface interval 
clear 

load srfstate 

stait(:,l) = state( : , 121) ; 
clear state 
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% run through thirty dives 

numbdives =30; idt = 1; T = 120; 
rpm = 550; nn = 0; 
for k2=l: numbdives 
for jj=l:T/idt 

numb = (k2-l-nn)*T + jj; 
plane = 0.02 * sin(pi*j j/60) ; 
inpt = [0; 0; plane; -plane; rpm]; 
oldstate = stait ( : , numb) ; 

stait ( : , numb+1) = auv2 (oldstate, inpt, idt) 
end 

if (k2==5) 

nn = k2 ; statemp=stait (:, numb+1) ; 
save statel stait 
clear stait 
stait (:,1) = statemp; 
end 

if (k2==10) 

nn = k2 ; statemp=stait (:, numb+1) ; 
save state2 stait 
clear stait 
stait(:,l) = statemp; 
end 

if (k2==15) 

nn = k2; statemp=stait( : ,numb+l) ; 
save state3 stait 
clear stait 
stait(;,l) = statemp; 
end 

if (k2==20) 

nn = k2 ; statemp=stait ( : , numb+l) ; 
save state4 stait 
clear stait 
stait(:,l) = statemp; 
end 

if (k2==25) 

nn = k2 ; statemp=sta it (:, numb+1) ; 
save state5 stait 
clear stait 
stait(:,l) = statemp; 
end 

if (k2==30) 

nn = k2; statemp=stait (:, numb+1) ; 
save state6 stait 
clear stait 
stait(:,l) = statemp; 
end 
end 
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Dlotdpth.m: 

% Steve Nagengast % plots the depth during first five dives 
% filename: plotdpth.m 

clear 

load statel % from auv2dive.m 

!del divedpth.met 
numbdives = 5; 

T = 120; 
idt = 1; 

for k2 = 1: numbdives 
for jj = l:T/idt 

numb = jj + (k2-l) * T; 
divedist (numb) = stait (7 , numb) ; 
divedepth (numb) = -stait (9 , numb) ; 
end 
end 

% plot results 
clg 

divemin =1/60:1/60: (numbdives) *120/60; 
n=numbdives ; 

subplot (2 11) , plot (divemin, divedepth) ; 

title ('DEPTH vs. TIME'); 

xlabel ( ' time, minutes'); 

ylabel ( 'depth, meters'); 

grid; 

subplot (212) , plot (divedist, divedepth) ; 

title ('DEPTH vs. DISTANCE'); 

xlabel ( ' distance , meters ' ) ; 

ylabel ( ' depth , meters ' ) ; 

grid 

meta divedpth; 



simlsurf .m; 

% Steve Nagengast % simulates INS/GPS Kalman filter for 

% filename: simlsurf. m % initial 2-minute surface interval 

% Estimates gyro drift errors and accelerometer biases 
% in body coordinates. Also estimates errors in attitude, 

% position, and velocity in earth coordinates. 

% initialize state, estimated state, and covariance matrices 

clear 

X = zeros (15,1) ; 
xhat = zeros (15,1); 
pr exhat = zeros (15,1); 
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% initial variances in P(0) matrix 



prep = zeros (15); 



prep (1,1) = 
prep (2, 2) = 
prep (3, 3) = 
prep (4, 4) = 
prep (5, 5) = 
prep (6, 6) = 
prep(7,7) = 
prep (8, 8) = 
prep (9, 9) = 
prep (10, 10) 
prep(ll, 11) 
prep (12, 12) 
prep (13,13) 
prep (14,14) 
prep (15,15) 



2.35e-7; 

2.35e-7; 

2.35e-7; 

400; 

400; 

400; 
le-8 ; 
le-8 ; 
le-8; 

= 2 . 3 5e-15 ; 
= 2.35e-15; 
= 2.35e-15; 
= le-8; 

= le-8; 

= le-8; 



% radians^2 
% radians^2 
% radians^2 
% meters^ 2 
% meters^2 
% meters^ 2 
% (meters/second) ^2 
% (meters/second) ^2 
% (meters/second) ^2 
% (radians/second) ^2 
% (radians/second) ^2 
% (radians/second) ^2 
% (meters/second^2) ^2 
% (meters/second^2) ^2 
% (meters/second^2) ^2 



H = zeros (9, 15); % H matrix 

for j=l:9 
H(j,j) = 1; 
end 



% initial transformation matrix from body to earth 
% coordinates 

% (assuming body is aligned with local-level frame) 



lat =0.64; % radians north (vicinity Monterey) 

long = pi - 2.13; % radians east (vicinity Monterey) 



Rbe = [ -sin ( lat) *cos( long) -sin (long) -cos (lat) *cos (long) 
-sin ( lat) *sin (long) cos (long) -cos (lat) *sin( long) 
cos(lat) 0 sin(lat) ] 



% initialize noise 



rand( 'normal' ) ; 
w = zeros (6,1) ; 

V = zeros (9 , 1) ; 

% standard deviations 

% radians/second 
% meters/ second 
% radians 
% meters 
% meters/ second 



sigmadrif t 

sigmabias 

sigmaatt 

sigmaposit 

sigmavel 



1.74e-6; 

6.1e-4; 

le-6; 

2.5; 

0.05; 
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% variances 



vardrift = 
varbias = 
varatt = 
varposit = 
varvel = 



sigmadrift''2 ; 
sigmabias^2 ; 
sigmaatt^2 ; 
sigmaposit^2 ; 
sigmavel^2 ; 



% Q matrix 



= zeros ( 15 ) ; 



Q(l,l) = 
Q(2,2) = 
Q(3,3) = 
Q(7,7) = 
Q(8,8) = 
Q(9,9) = 
Q(10,10) 
Q(ll,ll) 
Q(12,12) 
Q(13,13) 
Q(14,14) 
Q(15,15) 



2.35e-12 
2. 35e-12 
2.35e-12 
le-6 ; 
le-6 ; 
le-6 ; 

= 3e-12; 
= 3e-12; 
= 3e-12; 
= 3.7e-7 
= 3.7e-7 
= 3.7e-7 



/ 

/ 



1 

/ 

/ 



% R matrix 



R = zeros (9) ; 
R(l,l) = varatt; 

R ( 2 , 2 ) = varatt ; 
R(3,3) = varatt; 
R(4,4) = varposit; 
R(5,5) = varposit; 
R(6,6) = varposit; 
R(7,7) = varvel; 
R(8,8) = varvel; 
R(9,9) = varvel; 



% initialize submatrices for state dynamics matrix (F) 



omegaE = 0 . 7292115e-4 ; % radians/second 




skewE = [ 0 


omegaE 0 






-omegaE 


0 0 






0 


0 0 ]; 






Tgyro = -eye (3) / 


(3600*40) ; % 1 


/ (correlation 


time) 


Tacc = -eye (3) / 


(3600*40) ; % 1 


/ (correlation 


time) 


null3 = zeros (3) ; 








G = [null3 


null3 






null3 


null3 






null3 


null3 






eye(3) *vardrift null3 






null3 


eye (3) *varbias ]; 
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% initial surface interval (120 seconds = two minutes) 



load srf state % from auv2surf.m 

for k=l:120 

f = (state (:, k+1) - state (:,k)); 
dist(k) = state(7,k+i) ; 
depth(k) = -state (9, k+1 ) ; 



% update state dynamics matrix 



sbx = state (4, k+1) 
sby = state (5, k+1) 
sbz = state (6, k+1) 
skewb = [ 0 -sbz 
sbz 0 

-sby sbx 

Rbe + Rbe * 



Rbe(3,l) 
Rbe (3,2) 
Rbe (3,3) 
sby 
-sbx 
0 ]; 

skewb ; 



* omega E; 

* omegaE ; 

* omegaE; 



Rbe = 

f b = f ( 1 : 3 ) ; f e = Rbe * f b ; 
skewf = [ 0 fe(3) -fe(2) 

-fe(3) 0 fe(l) 

fe(2) -fe(l) 0 ]/ 

F = [skewE null3 null3 Rbe null3 
null3 null3 eye (3) null3 null3 
skewf null3 2*skewE null3 Rbe 
null3 null3 null3 Tgyro null3 
null3 null3 null3 null3 Tacc ] ; 



% approximate phi and gamma (idt = 1) 



F2 = F"2; 

F3 = F2 * F; 

F4 = F3 * F; 

phi = eye (15) + F + F2/2 + F3/6; 

gamma=(eye(15) + F/2 + F2/6 + F3/24 + F4/120) * G; 

% make some noise and create noisy measurements 

for n=l:6 

w(n) = rand; 
end 



V(l) 


= 


rand 


* 


varatt; 


v(2) 


= 


rand 


* 


varatt; 


V(3) 


= 


rand 


* 


varatt; 


V(4) 


= 


rand 


* 


varposit; 


v(5) 


= 


rand 


* 


varposit ; 


V(6) 


= 


rand 


* 


varposit; 


V(7) 


= 


rand 


* 


varvel; 


V(8) 


= 


rand 


* 


varvel; 


V(9) 


= 


rand 


* 


varvel ; 



x(:,k+l) = phi * x(:,k) + gamma * w; 
z = H * x( : ,k) + v; 
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% Kalman filter 



K = prep * H' * inv(H * prep * H' + R) ; 
xhat(:,k) = prexhat + K * (z - H * prexhat) ; 
p = (eye (15) - K * H) * prep; 
prep = phi * p * phi' + Q; 
prexhat = phi * xhat(:,k); 
countdown = 120 - k 
end 

% save and plot results from end of initial surface interval 
divex = x(:,121); stait = state (:, 121) ; 

save snl divex stait Rbe omegaE skewE Tgyro Tacc H prep R Q 
prexhat G 

save vars varatt varposit varvel 
plotl 



plotl.m: 

% Steve Nagengast % plots results of surface interval 

% filename: plotl.m 

!del meta?.met 

time = 1:120; min = 1/60:1/60:120/60; clg 

subplot (211) , plot (min, depth) ; title ('DEPTH vs. TIME'); 
xlabel ( 'time, minutes'); ylabel ( 'depth, meters'); grid 
subplot (2 12 ) , plot (dist, depth) ; title('DEPTH vs. DISTANCE'); 
xlabel ( 'distance, meters'); ylabel ( 'depth, meters'); 
grid; meta metal; clg 

subplot (221) , plot (time, X (1 , 1:120) , '-' , time,xhat (1, : ) , ' — ' ) 
title ( 'ATTITUDE ERROR'); xlabel (' time , seconds'); 
ylabel ( 'attitude error, radians'); grid 

subplot (222) , plot (time, X (2 , 1:120) , ' -' , time, xhat (2 , : ) , ' — ' ) 
title ('ATTITUDE ERROR'); xlabel (' time , seconds'); 
ylabel (' attitude error, radians'); grid 

subplot (223) , plot (time, X (3 , 1:120) , ' -' , time, xhat (3 , : ) , ' — ' ) 
title ( 'ATTITUDE ERROR'); xlabel ( 'time, seconds'); 
ylabel (' attitude error, radians'); grid; meta meta2; clg 

subplot (221) , plot (time, X (4 , 1:120) , '-' , time , xhat ( 4 , : ) , ' — ') 
title (' POSITION ERROR'); xlabel (' time , seconds'); 
ylabel ( 'position error, meters'); grid 

subplot (222) , plot (time, x(5, 1:120),'-' , time, xhat (5, : ) , ' — ' ) 
title ( 'POSITION ERROR'); xlabel ( 'time, seconds'); 
ylabel (' position error, meters'); grid 

subplot (223 ) , plot (time, X (6 , 1:120) , ' , time, xhat (6 , : ) , ' — ' ) 
title ( 'POSITION ERROR'); xlabel ( 'time, seconds'); 
ylabel ( 'position error, meters'); grid; meta meta3; clg 
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subplot (221) , plot (time, X (7 , 1:120),'-', time, xhat (7 , : ) , ' — ' ) 

title ( 'VELOCITY ERROR'); xlabel ( ' time, seconds'); 

ylabel ( 'velocity error, meters/second' ) ; grid 

subplot (222) , plot (time, x(8, 1:120),'-', time, xhat (8 , : ) , ' — ' ) 

title ('VELOCITY ERROR'); xlabel (' time , seconds'); 

ylabel ( 'velocity error, meters/second' ) ; grid 

subplot (22 3) , plot (time, x(9, 1:120) , '-' , time, xhat (9 , : ) , ' — ') 

title ( 'VELOCITY ERROR'); xlabel (' time , seconds'); 

ylabel ( 'velocity error, meters/second'); grid; meta meta4 ; 

clg 

subplot(221) , 

plot (time, x(10, 1:120) , '-' , time, xhat ( 10 , : ) , ' — ' ) 
title('GYRO DRIFT'); xlabel ( 'time, seconds'); 
ylabel('gyro drift, radians/second'); grid 
subplot (2 22) , 

plot ( time, x( 11, 1: 120) , '-' , time, xhat (11, : ) , ' — ') 
title('GYRO DRIFT'); xlabel ( 'time, seconds'); 
ylabel ('gyro drift, radians/second'); grid 
subplot (223) , 

plot (time, x( 12, 1:120) , '-' , time, xhat (12, : ) , ' — ') 
title('GYRO DRIFT'); xlabel (' time , seconds'); 
ylabel('gyro drift, radians/second'); grid; meta met5; clg 

subplot (221) , 

plot (time, x( 13 , 1:120) , '-' , time, xhat (13 , : ) , ' — ' ) 
title ( 'ACCELEROMETER BIAS' ) ; xlabel (' time, seconds'); 
ylabel('acc. bias, meters/ second ^^2 ') ; grid 
subplot (222) , 

plot (time , X (14 , 1:120) , '-' , time, xhat (14, : ) , ' — ' ) 
title ( 'ACCELEROMETER BIAS' ) ; xlabel (' time , seconds'); 
ylabel('acc. bias, meters/ second ^^2 ') ; grid 
subplot (223) , 

plot (time, X (15 ,1:120) , ' , time, xhat (15 , : ) , ' — ' ) 
title ('ACCELEROMETER BIAS') ; Xlabel ( 'time, seconds'); 
ylabel('acc. bias, meters/second^2 ' ) ; grid; meta met6; 



simdives.m; 

% Steve Nagengast % simulates INS/GPS thru thirty dives 
% filename: simdives.m 

% (uses average forces and rotation rates) 

% reload data from initial surface interval 



clear 
load snl 
load vars 
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% run through thirty dives 

numbdives = 30; idt = 1; T = 120; 
null3 = zeros(3); 
w = zeros (6,1) ; 

V = zeros (9 , 1) ; 

Q = Q * T; 

totalf = zeros (3,1); 
totalrot = zeros(3,l); 

load statel % from auv2dive.m 

state = stait; 

clear stait 

clear statel 

nn = 0; 

for k2=l : numbdives 
for jj=l:T/idt 

numb = (k2-l-nn) * T + jj; 

f = (state (:, numb+1) - state (:, numb) ) / idt 
totalrot = totalrot + state (4: 6, numb+1) ; 
totalf = totalf + f(l:3); 
end 

if (k2==5) 
nn = k2 ; 
clear state 
load state2 
state = stait; 
clear stait 
clear state2 
k2 = 5; 
nn = k2; 
end 

if (k2==10) 
nn = k2 ; 
clear state 
load state3 
state = stait; 
clear stait 
clear state3 
k2 = 10; 
nn = k2; 
end 

if (k2==15) 
nn = k2 ; 
clear state 
load state4 
state = stait; 
clear stait 
clear state4 
k2 = 15; 
nn = k2; 
end 

if (k2==20) 
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nn = k2 ; 
clear state 
load states 
state = stait; 
clear stait 
clear states 
k2 = 20; 
nn = k2 ; 
end 

if (k2==2S) 
nn = k2 ; 
clear state 
load States 
state = stait; 
clear stait 
clear states 
k2 = 2S; 
nn = k2 ; 
end 

% update transformation matrix and forces 

averot = totalrot / T; 
aveforce = totalf / T; 
sbx = averot (1) - Rbe(3,l) * omegaE; 

sby = averot (2) - Rbe(3,2) * omegaE; 

sbz = averot (3) - Rbe(3,3) * omegaE; 

skewb = [ 0 -sbz sby 

sbz 0 -sbx 

-sby sbx 0 ] ; 

Rbe = Rbe + Rbe * skewb * T; 
fe = Rbe * aveforce; 

% update state dynamics matrix 

skewf = [ 0 fe(3) -fe(2) 

-fe(3) 0 fe(l) 

fe(2) -fe(l) 0 ]; 

F = [skewE null3 null3 Rbe null3 
null3 null3 eye(3) null3 null3 
skewf null 3 2*skewE null 3 Rbe 
null3 null3 null3 Tgyro null3 
null3 null3 null3 null3 Tacc ] ; 

% approximate phi and gamma (T = 120) 

F2 = F'^2; 

F3 = F2 * F; 

F4 = F3 * F; 

phi = eye(lS) + F*T + F2*(T"2)/2 + F3*(T‘'3)/S; 
gamma=(eye(lS) + F*T/2 + F2*(T"2)/S + F3*(T''3)/24 + 

F4* (T''4) /120) *T*G; 
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make some system noise and update state 



for n=l:6 

w(n) = rand; 
end 

divex( : ,k2+l) = phi * divex(:,k2) + gamma * w; 



% make some measurement noise 



V(l) 


= 


rand 


* 


varatt ; 


V(2) 


= 


rand 


* 


varatt ; 


V(3) 


= 


rand 


* 


varatt; 


v(4) 


= 


rand 


* 


varposit ; 


V(5) 


=r 


rand 




varposit ; 


v(6) 


= 


rand 


* 


varposit; 


v(7) 


= 


rand 


* 


varvel ; 


v(8) 


= 


rand 


* 


varvel ; 


v(9) 


= 


rand 


* 


varvel ; 



% create noisy measurements and run through Kalman filter 

z = H * divex(:,k2) + v; 

K = prep * H' * inv(H * prep * + R) ; 

divexhat ( : , k2) = prexhat + K * (z - H * prexhat) ; 
p = (eye(15) - K * H) * prep; 
prep = phi * p * phi' + Q; 
prexhat = phi * divexhat (: ,k2) ; 
divecountdown = numbdives - k2 
end 

% save and plot results from dives 

save sn2 divex divexhat numbdives 
plot2 



plot2 .m: 

% Steve Nagengast % plots results from one hour of 

dives 

% filename: plot2.m 

clear; clg 
pack 
load sn2 
Idel met2?.met 

divemin = 2 : 2 : (numbdives) *2 ; 
n=numbdives; 
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subplot(221) , 

plot (divemin, di vex (1, 1 :n) , ' , divemin, divexhat (1 , : ) , ' — •) 
title ( 'ATTITUDE ERROR'); xlabel ( ' time, minutes'); 
y label ( 'attitude error, radians'); grid 
subplot (222) , 

plot (divemin, di vex (2, l:n) , , divemin,divexhat (2 , : ) , ' — ' ) 
title ( 'ATTITUDE ERROR'); xlabel (' time , minutes'); 
y label ( 'attitude error, radians'); grid 
subplot (223) , 

plot (divemin, divex (3 , 1 : n) , ' , divemin, divexhat (3 , : ) , ' — ' ) 
title ( 'ATTITUDE ERROR'); xlabel ( 'time, minutes'); 
ylabel ( 'attitude error, radians'); grid; meta met22; clg 

subplot (221) , 

plot(divemin,divex(4, l:n) , '-' ,divemin,divexhat(4, : ) , ' — ' ) 
title (' POSITION ERROR'); xlabel (' time , minutes'); 
ylabel ( 'position error, meters'); grid 
subplot (222) , 

plot (divemin, divex (5 , l:n) , '-' , divemin, divexhat ( 5 , : ) , ' — ' ) 
title ('POSITION ERROR'); xlabel (' time , minutes'); 
ylabel ( 'position error, meters'); grid 
subplot (223) , 

plot (divemin, divex (6 , l;n) , '-' , divemin, divexhat (6 , : ) , ' — ' ) 
title ('POSITION ERROR') ; xlabel (' time , minutes'); 
ylabel ( 'position error, meters'); grid; meta met23; clg 

subplot (221) , 

plot (divemin, divex (7 , 1 : n) , ' , divemin, divexhat (7 , : ) , ' — ' ) 
title ( 'VELOCITY ERROR'); xlabel ( 'time, minutes'); 
ylabel (' velocity error, meter s/second' ) ; grid 
subplot (222) , 

plot (divemin, divex (8 , l:n) , '-' , divemin, divexhat (8, : ) , ' — ' ) 
title ('VELOCITY ERROR'); xlabel ( 'time, minutes'); 
ylabel ( 'velocity error, meters/second'); grid 
subplot (223) , 

plot (divemin, divex (9, l:n) , '-' , divemin, divexhat (9, : ) , ' — ' ) 
title ( 'VELOCITY ERROR'); xlabel (' time , minutes'); 
ylabel ( 'velocity error, meters/ second' ) ; grid; meta met24; 
clg 

subplot (221) , 

plot (divemin, divex (10 , l:n) , '-' , divemin, divexhat (10, : ) , ' — ' ) 
title ('GYRO DRIFT'); xlabel ( 'time, minutes'); 
ylabel('gyro drift, radians/second'); grid 
subplot (222) , 

plot (divemin, divex (11, l;n) , '-' , divemin, divexhat (11, : ) , ' — ' ) 
title('GYRO DRIFT'); xlabel (' time , minutes'); 
ylabel ('gyro drift, radians/second'); grid 
subplot (223) , 

plot (divemin, divex (12,l:n) , '-' , divemin, divexhat (12 , : ) , ' — ' ) 
title ('GYRO DRIFT'); xlabel ( 'time, minutes'); 
ylabel('gyro drift, radians/second'); grid; meta met25; clg 
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subplot (221) , 

plot (divemin, divex (13,l:n) , , divemin, divexhat (13 , :) , ' — ') 
title ( 'ACCELEROMETER BIAS'); xlabel ( ' time , minutes'); 
ylabel('acc. bias, meters/second^2 ' ) ; grid 
subplot (222) , 

plot (divemin, divex (14,l:n) , divemin, divexhat ( 14 , : ) , ' — ' ) 
title ('ACCELEROMETER BIAS') ; xlabel ( ' time, minutes'); 
ylabel('acc. bias, meters/second^2 ' ) ; grid 
subplot (223) , 

plot (divemin, divex (15,l:n) , '-' , divemin, divexhat (15 , : ) , ' — ' ) 
title ( 'ACCELEROMETER BIAS' ) ; xlabel ( 'time, minutes'); 
ylabel('acc. bias, meters/second^2 ' ) ; grid; meta met26; 



esterr .m; 



% Steve Nagengast % plots error estimate errors 

after dives 
% filename: esterr. m 

clear; clg 
load sn2 
!del sn?.met 

divex = divex (:, 1 : numbdives) ; 
ester = divex - divexhat; 

subplot (221) , plot (ester (1, :)) ; title (' ATT. ERROR EST. 
ERROR' ) ; 

xlabel('dive number'); ylabel (' radians per second'); grid 
subplot(222) , plot(ester(2, :) ) ; title(' ATT. ERROR EST. 
ERROR') ; 

xlabel('dive number'); ylabel ( 'radians per second'); grid 
subplot (223 ) , plot (ester (3, :)) ; title (' ATT. ERROR EST. 
ERROR') ; 

xlabel('dive number'); ylabel ( 'radians per second'); grid; 
meta snl; clg 

subplot(221) , plot(ester(4, :) ) ; title ( 'POSITION ERROR 
ESTIMATE ERROR'); 

xlabel ('dive number'); ylabel ( 'meters' ) ; grid 
subplot (222) , plot (ester (5, :)) ; title ('POSITION ERROR 
ESTIMATE ERROR'); 

xlabel ('dive number'); ylabel ( 'meters' ) ; grid 
subplot(223) , plot (ester (6, :)) ; title ( 'POSITION ERROR 
ESTIMATE ERROR' ) ; 

xlabel('dive number'); ylabel ( 'meters' ) ; grid; meta sn2 ; clg 
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subplot (221) , plot (ester (7, :)) ; title ( 'VELOCITY ERROR 
ESTIMATE ERROR'); 

xlabel('dive number'); y label ( 'meters per second'); grid 
subplot (222) , plot (ester (8, :)) ; title ( 'VELOCITY ERROR 
ESTIMATE ERROR' ) ; 

xlabel('dive number'); ylabel ( 'meters per second'); grid 
subplot ( 223 ) , plot (ester (9, :)) ; title ( 'VELOCITY ERROR 
ESTIMATE ERROR' ) ; 

xlabel('dive number'); ylabel ( 'meters per second'); grid; 
meta sn3; clg 



subplot ( 221) , plot (ester ( 10 ,:)) ; title (' 
ERROR' ) ; 

xlabel('dive number'); ylabel (' radians per 
subplot ( 222) , plot (ester ( 11 ,:)) ; title(' 
ERROR') ; 

xlabel('dive number'); ylabel ( 'radians per 
subplot (223) , plot (ester ( 12 ,:)) ; title(' 
ERROR' ) ; 

xlabel('dive number'); ylabel (' radians per 
meta sn4; clg 



GYRO DRIFT EST. 

second'); grid 
GYRO DRIFT EST. 

second'); grid 
GYRO DRIFT EST. 

second'); grid; 



subplot (221) , plot (ester ( 13 ,:)) ; title (' 
ERROR' ) ; 

xlabel('dive number'); ylabel ( 'meters per 
grid 

subplot (222) , plot (ester (14, :)) ; title (' 
ERROR' ) ; 

xlabel('dive number'); ylabel ( 'meters per 
grid 

subplot (223 ) , plot (ester ( 15 ,:)) ; title (' 
ERROR' ) ; 

xlabel('dive number'); ylabel ( 'meters per 

grid; 

meta sn5; 



ACC. 


BIAS EST. 


second 


squared' ) ; 


ACC. 


BIAS EST. 


second 


squared' ) ; 


ACC. 


BIAS EST. 


second 


squared' ) ; 
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